#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <helper/helper_utils.h>

using namespace std;


// 运行示例： rosrun helper helper_lidar_info /scan_raw

/**
 * @description: 
 * @return {*}
 */
class LidarInfo{
    public:
        LidarInfo();
        LidarInfo(string lidar_topic);
        ~LidarInfo();

    private:
        ros::NodeHandle __nh;
        ros::Subscriber __lidarSub;

        // 雷达回调函数
        void LidarCallback(const sensor_msgs::LaserScanConstPtr& msg);

};


/**
 * @description: 
 * @return {*}
 */
LidarInfo::LidarInfo(){
    __lidarSub = __nh.subscribe("/scan", 1, &LidarInfo::LidarCallback, this);
}

LidarInfo::LidarInfo(string lidar_topic){
    __lidarSub = __nh.subscribe(lidar_topic, 1, &LidarInfo::LidarCallback, this);
}

/**
 * @description: 
 * @return {*}
 */
LidarInfo::~LidarInfo(){
    
}


/**
 * @description: 
 * @param {LaserScanConstPtr&} msg
 * @return {*}
 */
void LidarInfo::LidarCallback(const sensor_msgs::LaserScanConstPtr& msg){
    // 打印雷达数据
    ROS_INFO("----------------------[LidarCallback]----------------------");
    // 打印正向距离
    int front_idx = static_cast<int>((0 - msg->angle_min) / msg->angle_increment);
    ROS_INFO("[%s] angle_range: [%0.3f, %0.3f] | [%0.3f, %0.3f], rangse size: %ld, front_idx: %d, front_distance: %0.3f", 
        helper::stamp_to_standard(msg->header.stamp.sec).c_str(),
        msg->angle_min, msg->angle_max,
        helper::Rad2Deg(msg->angle_min), helper::Rad2Deg(msg->angle_max), 
        msg->ranges.size(), 
        front_idx, 
        msg->ranges[front_idx]
    ); 
}


/**
 * @description: 
 * @param {int} argc
 * @param {char} *
 * @return {*}
 */
int main(int argc, char **argv ){

    // 打印参数
    for(int i=0; i<argc; i++){
        std::cout << "i: " << i << ", data: " << argv[i] << std::endl;
    }
    if(argc < 2){
        std::cout << "please set lidar topic."  << std::endl;
        return 1;
    }
    ros::init(argc, argv, "helper_lidar_info");
    ROS_INFO("helper_lidar_info initialization ......");
    ros::NodeHandle nh;
    LidarInfo lidar_info(argv[1]);
    ros::spin();
    return 0;
}